#include "rack.h"
#include <Arduino.h>

Rack::Rack()
{
    //ctor
}

void Rack::setMean(double x)
{
    mean = x;
}

void Rack::setPwm(int pwm)
{
    Rack::pwm = pwm;
    analogWrite(pwmPin, pwm);
}

void Rack::attachMotor(int dir1Pin,int dir2Pin, int pwmPin)
{
    pinMode(dir1Pin, OUTPUT);
    pinMode(dir2Pin, OUTPUT);
    pinMode(pwmPin, OUTPUT);
    digitalWrite(dir1,HIGH);    digitalWrite(dir2,HIGH);
    analogWrite(pwm,Rack::pwm);
    dir1 = dir1Pin;
    dir2 = dir2Pin;
    Rack::pwmPin = pwmPin;
}

void Rack::attachVariac(int pinNo)
{
    pinMode(pinNo, INPUT);
    pin = pinNo;
}

void Rack::moveAbsolute(int x)
{
    curPos = analogRead(pin);
    if(curPos < x - 3 && movingStatus != 1)                  // 3 to remove noise effect
    {                                                        // which results in astability
        digitalWrite(dir1, LOW);   digitalWrite(dir2, HIGH);
        movingStatus = 1;
    }
    if(curPos > x + 3 && movingStatus != -1)
    {
        digitalWrite(dir1, HIGH);   digitalWrite(dir2, LOW);
        movingStatus = -1;
    }
    if(curPos < x+3 && curPos > x - 3 && movingStatus != 0)
    {
        stop();
        movingStatus = 0;
    }
}

void Rack::moveRelative(int x)
{
    moveAbsolute(mean + x);
}

void Rack::moveCmAbsolute(int cm)
{
    moveAbsolute( cm*c2rRatio + mean);
}

void Rack::moveCmRelative(int cm)
{
    moveAbsolute( cm*c2rRatio + curPos);    
}

void Rack::setAsMean()
{
    mean = curPos;
}

int Rack::isMoving()
{
    return movingStatus;
}

int Rack::position()
{
    return curPos;
}

void Rack::stop()
{
    digitalWrite(dir1,HIGH);
    digitalWrite(dir2,HIGH);
}
